fusion.h File Reference

Include dependency graph for fusion.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Macros
#define	FLPFSECS_1DOF_P_BASIC   1.5F
#define	FLPFSECS_3DOF_G_BASIC   1.0F
#define	FLPFSECS_3DOF_B_BASIC   7.0F
#define	FMAX_6DOF_GY_BPL   7.0F
#define	FMAX_9DOF_GBY_BPL   7.0F
COMPUTE_6DOF_GB_BASIC constants
#define	FLPFSECS_6DOF_GB_BASIC   7.0F
COMPUTE_6DOF_GY_KALMAN constants
#define	FQVY_6DOF_GY_KALMAN   2E2
#define	FQVG_6DOF_GY_KALMAN   1.2E-3
#define	FQWB_6DOF_GY_KALMAN   2E-2F
#define	FMIN_6DOF_GY_BPL   -7.0F
COMPUTE_9DOF_GBY_KALMAN constants
gyro sensor noise covariance units deg^2 increasing this parameter improves convergence to the geomagnetic field
#define	FQVY_9DOF_GBY_KALMAN   2E2
#define	FQVG_9DOF_GBY_KALMAN   1.2E-3
#define	FQVB_9DOF_GBY_KALMAN   5E0
#define	FQWB_9DOF_GBY_KALMAN   2E-2F
#define	FMIN_9DOF_GBY_BPL   -7.0F

Functions
Fusion Function Prototypes
These functions comprise the core of the basic sensor fusion functions excluding magnetic and acceleration calibration. Parameter descriptions are not included here, as details are provided in sensor_fusion.h.
void	fInitializeFusion (SensorFusionGlobals *sfg)
void	fFuseSensors (struct SV_1DOF_P_BASIC *pthisSV_1DOF_P_BASIC, struct SV_3DOF_G_BASIC *pthisSV_3DOF_G_BASIC, struct SV_3DOF_B_BASIC *pthisSV_3DOF_B_BASIC, struct SV_3DOF_Y_BASIC *pthisSV_3DOF_Y_BASIC, struct SV_6DOF_GB_BASIC *pthisSV_6DOF_GB_BASIC, struct SV_6DOF_GY_KALMAN *pthisSV_6DOF_GY_KALMAN, struct SV_9DOF_GBY_KALMAN *pthisSV_9DOF_GBY_KALMAN, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct GyroSensor *pthisGyro, struct PressureSensor *pthisPressure, struct MagCalibration *pthisMagCal)
void	fInit_1DOF_P_BASIC (struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure, float flpftimesecs)
void	fInit_3DOF_G_BASIC (struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel, float flpftimesecs)
void	fInit_3DOF_B_BASIC (struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag, float flpftimesecs)
void	fInit_3DOF_Y_BASIC (struct SV_3DOF_Y_BASIC *pthisSV)
void	fInit_6DOF_GB_BASIC (struct SV_6DOF_GB_BASIC *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, float flpftimesecs)
void	fInit_6DOF_GY_KALMAN (struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro)
void	fInit_9DOF_GBY_KALMAN (struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct GyroSensor *pthisGyro, struct MagCalibration *pthisMagCal)
void	fRun_1DOF_P_BASIC (struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure)
void	fRun_3DOF_G_BASIC (struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel)
void	fRun_3DOF_B_BASIC (struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag)
void	fRun_3DOF_Y_BASIC (struct SV_3DOF_Y_BASIC *pthisSV, struct GyroSensor *pthisGyro)
void	fRun_6DOF_GB_BASIC (struct SV_6DOF_GB_BASIC *pthisSV, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel)
void	fRun_6DOF_GY_KALMAN (struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro)
void	fRun_9DOF_GBY_KALMAN (struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct GyroSensor *pthisGyro, struct MagCalibration *pthisMagCal)

Detailed Description

This file can be used to "tune" the performance of specific algorithms within the sensor fusion library. It also defines the lower level function definitions for specific algorithms. Normally, the higher level hooks in sensor_fusion.h will be used, and those shown here will be left alone.

Definition in file fusion.h.

Macro Definition Documentation

#define FLPFSECS_1DOF_P_BASIC   1.5F

Referenced by fRun_1DOF_P_BASIC().

#define FLPFSECS_3DOF_B_BASIC   7.0F

Referenced by fRun_3DOF_B_BASIC().

#define FLPFSECS_3DOF_G_BASIC   1.0F

Referenced by fRun_3DOF_G_BASIC().

#define FLPFSECS_6DOF_GB_BASIC   7.0F

Definition at line 57 of file fusion.h.

Referenced by fRun_6DOF_GB_BASIC().

#define FMAX_6DOF_GY_BPL   7.0F

Referenced by fInit_6DOF_GY_KALMAN().

#define FMAX_9DOF_GBY_BPL   7.0F

Referenced by fInit_9DOF_GBY_KALMAN(), and fRun_6DOF_GY_KALMAN().

#define FMIN_6DOF_GY_BPL   -7.0F

minimum permissible power on gyro offsets (deg/s)

Definition at line 65 of file fusion.h.

Referenced by fInit_6DOF_GY_KALMAN().

#define FMIN_9DOF_GBY_BPL   -7.0F

minimum permissible power on gyro offsets (deg/s)

Definition at line 77 of file fusion.h.

Referenced by fInit_9DOF_GBY_KALMAN(), and fRun_6DOF_GY_KALMAN().

#define FQVB_9DOF_GBY_KALMAN   5E0

magnetometer sensor noise variance units uT^2 defining minimum deviation from geomagnetic sphere.

Definition at line 75 of file fusion.h.

Referenced by fRun_6DOF_GY_KALMAN().

#define FQVG_6DOF_GY_KALMAN   1.2E-3

accelerometer sensor noise variance units g^2

Definition at line 63 of file fusion.h.

Referenced by fRun_6DOF_GY_KALMAN().

#define FQVG_9DOF_GBY_KALMAN   1.2E-3

accelerometer sensor noise variance units g^2 defining minimum deviation from 1g sphere

Definition at line 74 of file fusion.h.

Referenced by fRun_6DOF_GY_KALMAN().

#define FQVY_6DOF_GY_KALMAN   2E2

gyro sensor noise variance units (deg/s)^2

Definition at line 62 of file fusion.h.

Referenced by fInit_6DOF_GY_KALMAN().

#define FQVY_9DOF_GBY_KALMAN   2E2

gyro sensor noise variance units (deg/s)^2

Definition at line 73 of file fusion.h.

Referenced by fInit_9DOF_GBY_KALMAN().

#define FQWB_6DOF_GY_KALMAN   2E-2F

gyro offset random walk units (deg/s)^2

Definition at line 64 of file fusion.h.

Referenced by fInit_6DOF_GY_KALMAN().

#define FQWB_9DOF_GBY_KALMAN   2E-2F

gyro offset random walk units (deg/s)^2

Definition at line 76 of file fusion.h.

Referenced by fInit_6DOF_GY_KALMAN(), and fInit_9DOF_GBY_KALMAN().

Function Documentation

void fFuseSensors	(	struct SV_1DOF_P_BASIC *	pthisSV_1DOF_P_BASIC,
		struct SV_3DOF_G_BASIC *	pthisSV_3DOF_G_BASIC,
		struct SV_3DOF_B_BASIC *	pthisSV_3DOF_B_BASIC,
		struct SV_3DOF_Y_BASIC *	pthisSV_3DOF_Y_BASIC,
		struct SV_6DOF_GB_BASIC *	pthisSV_6DOF_GB_BASIC,
		struct SV_6DOF_GY_KALMAN *	pthisSV_6DOF_GY_KALMAN,
		struct SV_9DOF_GBY_KALMAN *	pthisSV_9DOF_GBY_KALMAN,
		struct AccelSensor *	pthisAccel,
		struct MagSensor *	pthisMag,
		struct GyroSensor *	pthisGyro,
		struct PressureSensor *	pthisPressure,
		struct MagCalibration *	pthisMagCal
	)

Definition at line 86 of file fusion.c.

Referenced by runFusion().

{
    // 1DOF Pressure: call the low pass filter algorithm
#if F_1DOF_P_BASIC
    if (pthisSV_1DOF_P_BASIC)
    {
        ARM_systick_start_ticks(&(pthisSV_1DOF_P_BASIC->systick));
        fRun_1DOF_P_BASIC(pthisSV_1DOF_P_BASIC, pthisPressure);
        pthisSV_1DOF_P_BASIC->systick = ARM_systick_elapsed_ticks(pthisSV_1DOF_P_BASIC->systick);
    }
#endif

    // 3DOF Accel Basic: call the tilt algorithm
#if F_3DOF_G_BASIC
    if (pthisSV_3DOF_G_BASIC)
    {
        ARM_systick_start_ticks(&(pthisSV_3DOF_G_BASIC->systick));
        fRun_3DOF_G_BASIC(pthisSV_3DOF_G_BASIC, pthisAccel);
        pthisSV_3DOF_G_BASIC->systick = ARM_systick_elapsed_ticks(pthisSV_3DOF_G_BASIC->systick);
    }
#endif

    // 3DOF Magnetometer Basic: call the 2D vehicle compass algorithm
#if F_3DOF_B_BASIC
    if (pthisSV_3DOF_B_BASIC)
    {
        ARM_systick_start_ticks(&(pthisSV_3DOF_B_BASIC->systick));
        fRun_3DOF_B_BASIC(pthisSV_3DOF_B_BASIC, pthisMag);
        pthisSV_3DOF_B_BASIC->systick = ARM_systick_elapsed_ticks(pthisSV_3DOF_B_BASIC->systick);
    }
#endif

    // 3DOF Gyro Basic: call the gyro integration algorithm
#if F_3DOF_Y_BASIC
    if (pthisSV_3DOF_Y_BASIC)
    {
        ARM_systick_start_ticks(&(pthisSV_3DOF_Y_BASIC->systick));
        fRun_3DOF_Y_BASIC(pthisSV_3DOF_Y_BASIC, pthisGyro);
        pthisSV_3DOF_Y_BASIC->systick = ARM_systick_elapsed_ticks(pthisSV_3DOF_Y_BASIC->systick);
    }
#endif

    // 6DOF Accel / Mag: Basic: call the eCompass orientation algorithm
#if F_6DOF_GB_BASIC
    if (pthisSV_6DOF_GB_BASIC)
    {
        ARM_systick_start_ticks(&(pthisSV_6DOF_GB_BASIC->systick));
        fRun_6DOF_GB_BASIC(pthisSV_6DOF_GB_BASIC, pthisMag, pthisAccel);
        pthisSV_6DOF_GB_BASIC->systick = ARM_systick_elapsed_ticks(pthisSV_6DOF_GB_BASIC->systick);
    }
#endif

    // 6DOF Accel / Gyro: call the Kalman filter orientation algorithm
#if F_6DOF_GY_KALMAN
    if (pthisSV_6DOF_GY_KALMAN)
    {
        ARM_systick_start_ticks(&(pthisSV_6DOF_GY_KALMAN->systick));
        fRun_6DOF_GY_KALMAN(pthisSV_6DOF_GY_KALMAN, pthisAccel, pthisGyro);
        pthisSV_6DOF_GY_KALMAN->systick = ARM_systick_elapsed_ticks(pthisSV_6DOF_GY_KALMAN->systick);
    }
#endif

    // 9DOF Accel / Mag / Gyro: call the Kalman filter orientation algorithm
#if F_9DOF_GBY_KALMAN
    if (pthisSV_9DOF_GBY_KALMAN)
    {
        ARM_systick_start_ticks(&(pthisSV_9DOF_GBY_KALMAN->systick));
        fRun_9DOF_GBY_KALMAN(pthisSV_9DOF_GBY_KALMAN, pthisAccel, pthisMag,
                             pthisGyro, pthisMagCal);
        pthisSV_9DOF_GBY_KALMAN->systick = ARM_systick_elapsed_ticks(pthisSV_9DOF_GBY_KALMAN->systick);
    }
#endif
    return;
}

Here is the call graph for this function:

Here is the caller graph for this function:

void fInit_1DOF_P_BASIC	(	struct SV_1DOF_P_BASIC *	pthisSV,
		struct PressureSensor *	pthisPressure,
		float	flpftimesecs
	)

Definition at line 172 of file fusion.c.

Referenced by fRun_1DOF_P_BASIC().

{
    // set algorithm sampling interval (typically 25Hz) and low pass filter
    // Note: the MPL3115 sensor only updates its output every 1s and is therefore repeatedly oversampled at 25Hz
    // but executing the exponenial filter at the 25Hz rate also performs an interpolation giving smoother output.
    // set algorithm sampling interval (typically 25Hz)
    pthisSV->fdeltat = 1.0F / (float) FUSION_HZ;

    // set low pass filter constant with maximum value 1.0 (all pass) decreasing to 0.0 (increasing low pass)
    if (flpftimesecs > pthisSV->fdeltat)
        pthisSV->flpf = pthisSV->fdeltat / flpftimesecs;
    else
        pthisSV->flpf = 1.0F;

    // initialize the low pass filters to current measurement
    pthisSV->fLPH = pthisPressure->fH;
    pthisSV->fLPT = pthisPressure->fT;

    // clear the reset flag
    pthisSV->resetflag = false;

    return;
}   // end fInit_1DOF_P_BASIC

Here is the caller graph for this function:

void fInit_3DOF_B_BASIC	(	struct SV_3DOF_B_BASIC *	pthisSV,
		struct MagSensor *	pthisMag,
		float	flpftimesecs
	)

Definition at line 228 of file fusion.c.

Referenced by fRun_3DOF_B_BASIC().

{
    // set algorithm sampling interval (typically 25Hz)
    pthisSV->fdeltat = 1.0F / (float) FUSION_HZ;

    // set low pass filter constant with maximum value 1.0 (all pass) decreasing to 0.0 (increasing low pass)
    if (flpftimesecs > pthisSV->fdeltat)
        pthisSV->flpf = pthisSV->fdeltat / flpftimesecs;
    else
        pthisSV->flpf = 1.0F;

    // initialize the low pass filtered magnetometer orientation matrix and quaternion using fBc
#if THISCOORDSYSTEM == NED
    f3DOFMagnetometerMatrixNED(pthisSV->fLPR, pthisMag->fBc);
#elif THISCOORDSYSTEM == ANDROID
    f3DOFMagnetometerMatrixAndroid(pthisSV->fLPR, pthisMag->fBc);
#else // WIN8
    f3DOFMagnetometerMatrixWin8(pthisSV->fLPR, pthisMag->fBc);
#endif
    fQuaternionFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPq));

    // update the default quaternion type supported to the most sophisticated
    //if (globals.DefaultQuaternionPacketType < Q3M)
    //globals.QuaternionPacketType = globals.DefaultQuaternionPacketType = Q3M;
    // clear the reset flag
    pthisSV->resetflag = false;

    return;
}   // end fInit_3DOF_B_BASIC

Here is the call graph for this function:

Here is the caller graph for this function:

void fInit_3DOF_G_BASIC	(	struct SV_3DOF_G_BASIC *	pthisSV,
		struct AccelSensor *	pthisAccel,
		float	flpftimesecs
	)

Definition at line 197 of file fusion.c.

Referenced by fRun_3DOF_G_BASIC().

{
    // set algorithm sampling interval (typically 25Hz)
    pthisSV->fdeltat = 1.0F / (float) FUSION_HZ;

    // set low pass filter constant with maximum value 1.0 (all pass) decreasing to 0.0 (increasing low pass)
    if (flpftimesecs > pthisSV->fdeltat)
        pthisSV->flpf = pthisSV->fdeltat / flpftimesecs;
    else
        pthisSV->flpf = 1.0F;

    // apply the tilt estimation algorithm to initialize the low pass orientation matrix and quaternion
#if THISCOORDSYSTEM == NED
    f3DOFTiltNED(pthisSV->fLPR, pthisAccel->fGc);
#elif THISCOORDSYSTEM == ANDROID
    f3DOFTiltAndroid(pthisSV->fLPR, pthisAccel->fGc);
#else // WIN8
    f3DOFTiltWin8(pthisSV->fLPR, pthisAccel->fGc);
#endif
    fQuaternionFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPq));

    // update the default quaternion type supported to the most sophisticated
    //if (globals.DefaultQuaternionPacketType < Q3)
    //globals.QuaternionPacketType = globals.DefaultQuaternionPacketType = Q3;
    // clear the reset flag
    pthisSV->resetflag = false;

    return;
}   // end fInit_3DOF_G_BASIC

Here is the call graph for this function:

Here is the caller graph for this function:

void fInit_3DOF_Y_BASIC

(

struct SV_3DOF_Y_BASIC *

pthisSV

)

Definition at line 259 of file fusion.c.

Referenced by fRun_3DOF_Y_BASIC().

{
    // compute the sampling time intervals (secs)
    pthisSV->fdeltat = 1.0F / (float) FUSION_HZ;

    // initialize orientation estimate to flat
    f3x3matrixAeqI(pthisSV->fR);
    fqAeq1(&(pthisSV->fq));

    // update the default quaternion type supported to the most sophisticated
    //if (globals.DefaultQuaternionPacketType < Q3G)
    //globals.QuaternionPacketType = globals.DefaultQuaternionPacketType = Q3G;
    // clear the reset flag
    pthisSV->resetflag = false;

    return;
}   // end fInit_3DOF_Y_BASIC

Here is the call graph for this function:

Here is the caller graph for this function:

void fInit_6DOF_GB_BASIC	(	struct SV_6DOF_GB_BASIC *	pthisSV,
		struct AccelSensor *	pthisAccel,
		struct MagSensor *	pthisMag,
		float	flpftimesecs
	)

Definition at line 277 of file fusion.c.

Referenced by fRun_6DOF_GB_BASIC().

{
    float ftmp;

    // set algorithm sampling interval (typically 25Hz)
    pthisSV->fdeltat = 1.0F / (float) FUSION_HZ;

    // set low pass filter constant with maximum value 1.0 (all pass) decreasing to 0.0 (increasing low pass)
    if (flpftimesecs > pthisSV->fdeltat)
        pthisSV->flpf = pthisSV->fdeltat / flpftimesecs;
    else
        pthisSV->flpf = 1.0F;

    // initialize the instantaneous orientation matrix, inclination angle and quaternion
#if THISCOORDSYSTEM == NED
    feCompassNED(pthisSV->fLPR, &(pthisSV->fLPDelta), &ftmp, &ftmp, pthisMag->fBc, pthisAccel->fGc, &ftmp, &ftmp );
#elif THISCOORDSYSTEM == ANDROID
    feCompassAndroid(pthisSV->fLPR, &(pthisSV->fLPDelta), &ftmp, &ftmp, pthisMag->fBc, pthisAccel->fGc, &ftmp, &ftmp);
#else // WIN8
    feCompassWin8(pthisSV->fLPR, &(pthisSV->fLPDelta), &ftmp, &ftmp, pthisMag->fBc, pthisAccel->fGc, &ftmp, &ftmp);
#endif
    fQuaternionFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPq));

    // update the default quaternion type supported to the most sophisticated
    //if (globals.DefaultQuaternionPacketType < Q6MA)
    //    globals.QuaternionPacketType = globals.DefaultQuaternionPacketType = Q6MA;

    // clear the reset flag
    pthisSV->resetflag = false;

    return;
}   // end fInit_6DOF_GB_BASIC

Here is the call graph for this function:

Here is the caller graph for this function:

void fInit_6DOF_GY_KALMAN	(	struct SV_6DOF_GY_KALMAN *	pthisSV,
		struct AccelSensor *	pthisAccel,
		struct GyroSensor *	pthisGyro
	)

Definition at line 313 of file fusion.c.

Referenced by fRun_6DOF_GY_KALMAN().

{
    float   *pFlash;    // pointer to flash float words
    int8    i;          // loop counter

    // compute and store useful product terms to save floating point calculations later
    pthisSV->fdeltat = 1.0F / (float) FUSION_HZ;
    pthisSV->fQwbOver3 = FQWB_6DOF_GY_KALMAN / 3.0F;
    pthisSV->fAlphaOver2 = FPIOVER180 * pthisSV->fdeltat / 2.0F;
    pthisSV->fAlphaSqOver4 = pthisSV->fAlphaOver2 * pthisSV->fAlphaOver2;
    pthisSV->fAlphaQwbOver6 = pthisSV->fAlphaOver2 * pthisSV->fQwbOver3;
    pthisSV->fAlphaSqQvYQwbOver12 = pthisSV->fAlphaSqOver4 * (FQVY_6DOF_GY_KALMAN + FQWB_6DOF_GY_KALMAN) / 3.0F;
    pthisSV->fMaxGyroOffsetChange = sqrtf(fabs(FQWB_9DOF_GBY_KALMAN)) / (float) FUSION_HZ;

    // zero the a posteriori gyro offset and error vectors
    for (i = CHX; i <= CHZ; i++)
    {
        pthisSV->fqgErrPl[i] = 0.0F;
        pthisSV->fbErrPl[i] = 0.0F;
    }

    // check to see if a gyro calibration exists in flash
    // the standard value for erased flash is 0xFF in each byte but for portability check against 0x12345678
    pFlash = (float *) (CALIBRATION_NVM_ADDR + GYRO_NVM_OFFSET);
    if (*((uint32 *) pFlash++) == 0x12345678)
    {
        // copy the gyro calibration from flash into the state vector
        for (i = CHX; i <= CHZ; i++) pthisSV->fbPl[i] = *(pFlash++);
    }
    else
    {
        // set the gyro offset to the current measurement if within limits
        for (i = CHX; i <= CHZ; i++)
        {
            if ((pthisGyro->fYs[i] >= FMIN_6DOF_GY_BPL) &&
                (pthisGyro->fYs[i] <= FMAX_6DOF_GY_BPL))
                pthisSV->fbPl[i] = pthisGyro->fYs[i];
            else
                pthisSV->fbPl[i] = 0.0F;
        }
    }

    // initialize the a posteriori orientation state vector to the tilt orientation
#if THISCOORDSYSTEM == NED
    f3DOFTiltNED(pthisSV->fRPl, pthisAccel->fGc);
#elif THISCOORDSYSTEM == ANDROID
    f3DOFTiltAndroid(pthisSV->fRPl, pthisAccel->fGc);
#else // Win8
    f3DOFTiltWin8(pthisSV->fRPl, pthisAccel->fGc);
#endif
    fQuaternionFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fqPl));

    // update the default quaternion type supported to the most sophisticated
    //if (globals.DefaultQuaternionPacketType < Q6AG)
    //globals.QuaternionPacketType = globals.DefaultQuaternionPacketType = Q6AG;

    // clear the reset flag
    pthisSV->resetflag = false;

    return;
}                       // end fInit_6DOF_GY_KALMAN

Here is the call graph for this function:

Here is the caller graph for this function:

void fInit_9DOF_GBY_KALMAN	(	struct SV_9DOF_GBY_KALMAN *	pthisSV,
		struct AccelSensor *	pthisAccel,
		struct MagSensor *	pthisMag,
		struct GyroSensor *	pthisGyro,
		struct MagCalibration *	pthisMagCal
	)

Definition at line 378 of file fusion.c.

Referenced by fRun_6DOF_GY_KALMAN().

{
    float ftmp;// scratch
    float *pFlash;// pointer to flash float words
    int8 i;// loop counter

    // compute and store useful product terms to save floating point calculations later
    pthisSV->fdeltat = 1.0F / (float) FUSION_HZ;
    pthisSV->fgdeltat = GTOMSEC2 * pthisSV->fdeltat;
    pthisSV->fQwbOver3 = FQWB_9DOF_GBY_KALMAN / 3.0F;
    pthisSV->fAlphaOver2 = FPIOVER180 * pthisSV->fdeltat / 2.0F;
    pthisSV->fAlphaSqOver4 = pthisSV->fAlphaOver2 * pthisSV->fAlphaOver2;
    pthisSV->fAlphaQwbOver6 = pthisSV->fAlphaOver2 * pthisSV->fQwbOver3;
    pthisSV->fAlphaSqQvYQwbOver12 = pthisSV->fAlphaSqOver4 * (FQVY_9DOF_GBY_KALMAN + FQWB_9DOF_GBY_KALMAN) / 3.0F;
    pthisSV->fMaxGyroOffsetChange = sqrtf(fabs(FQWB_9DOF_GBY_KALMAN)) / (float)FUSION_HZ;

    // zero the a posteriori error vectors and inertial outputs
    for (i = CHX; i <= CHZ; i++) {
        pthisSV->fqgErrPl[i] = 0.0F;
        pthisSV->fqmErrPl[i] = 0.0F;
        pthisSV->fbErrPl[i] = 0.0F;
        pthisSV->fVelGl[i] = 0.0F;
        pthisSV->fDisGl[i] = 0.0F;
    }

    // check to see if a gyro calibration exists in flash
    // the standard value for erased flash is 0xFF in each byte but for portability check against 0x12345678
    pFlash = (float *) (CALIBRATION_NVM_ADDR + GYRO_NVM_OFFSET);
    if (*((uint32*) pFlash++) == 0x12345678) {
    // copy the gyro calibration from flash into the state vector
        for (i = CHX; i <= CHZ; i++)
            pthisSV->fbPl[i] = *(pFlash++);
    } else {
        // set the gyro offset to the current measurement if within limits
        for (i = CHX; i <= CHZ; i++) {
            if ((pthisGyro->fYs[i] >= FMIN_9DOF_GBY_BPL) && (pthisGyro->fYs[i] <= FMAX_9DOF_GBY_BPL))
                pthisSV->fbPl[i] = pthisGyro->fYs[i];
            else
                pthisSV->fbPl[i] = 0.0F;
        }
     }

    // initialize the posteriori orientation state vector to the instantaneous eCompass orientation
    pthisSV->iFirstAccelMagLock = false;
#if THISCOORDSYSTEM == NED
    feCompassNED(pthisSV->fRPl, &(pthisSV->fDeltaPl), &(pthisSV->fsinDeltaPl), &(pthisSV->fcosDeltaPl),
        pthisMag->fBc, pthisAccel->fGc, &ftmp, &ftmp);
#elif THISCOORDSYSTEM == ANDROID
    feCompassAndroid(pthisSV->fRPl, &(pthisSV->fDeltaPl),  &(pthisSV->fsinDeltaPl), &(pthisSV->fcosDeltaPl),
        pthisMag->fBc, pthisAccel->fGc, &ftmp, &ftmp);
#else  // WIN8
    feCompassWin8(pthisSV->fRPl, &(pthisSV->fDeltaPl), &(pthisSV->fsinDeltaPl), &(pthisSV->fcosDeltaPl),
        pthisMag->fBc, pthisAccel->fGc, &ftmp, &ftmp);
#endif
    fQuaternionFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fqPl));

    // clear the reset flag
    pthisSV->resetflag = false;

    return;
} // end fInit_9DOF_GBY_KALMAN

Here is the call graph for this function:

Here is the caller graph for this function:

void fInitializeFusion

(

SensorFusionGlobals *

sfg

)

Definition at line 51 of file fusion.c.

Referenced by DecodeCommandBytes(), and initializeFusionEngine().

{
    // reset the quaternion type to the default packet type
    // sfg->pControlSubsystem->QuaternionPacketType = sfg->pControlSubsystem->DefaultQuaternionPacketType;

    // force a reset of all the algorithms next time they execute
    // the initialization will result in the default and current quaternion being set to the most sophisticated
    // algorithm supported by the build
#if F_1DOF_P_BASIC
    sfg->SV_1DOF_P_BASIC.resetflag    = true;
#endif
#if F_3DOF_B_BASIC
    sfg->SV_3DOF_B_BASIC.resetflag    = true;
#endif
#if F_3DOF_G_BASIC
    sfg->SV_3DOF_G_BASIC.resetflag    = true;
#endif
#if F_3DOF_Y_BASIC
    sfg->SV_3DOF_Y_BASIC.resetflag    = true;
#endif
#if F_6DOF_GB_BASIC
    sfg->SV_6DOF_GB_BASIC.resetflag   = true;
#endif
#if F_6DOF_GY_KALMAN
    sfg->SV_6DOF_GY_KALMAN.resetflag  = true;
#endif
#if F_9DOF_GBY_KALMAN
    sfg->SV_9DOF_GBY_KALMAN.resetflag = true;
#endif

    // reset the loop counter to zero for first iteration
    sfg->loopcounter = 0;
    return;
}

Here is the caller graph for this function:

void fRun_1DOF_P_BASIC	(	struct SV_1DOF_P_BASIC *	pthisSV,
		struct PressureSensor *	pthisPressure
	)

Definition at line 447 of file fusion.c.

Referenced by fFuseSensors().

{
    // if requested, do a reset and return
    if (pthisSV->resetflag)
    {
        fInit_1DOF_P_BASIC(pthisSV, pthisPressure, FLPFSECS_1DOF_P_BASIC);
        return;
    }

    // exponentially low pass filter the pressure and temperature.
    // when executed at (typically) 25Hz, this filter interpolates the raw signals which are
    // output every 1s in Auto Acquisition mode.
    pthisSV->fLPH += pthisSV->flpf * (pthisPressure->fH - pthisSV->fLPH);
    pthisSV->fLPT += pthisSV->flpf * (pthisPressure->fT - pthisSV->fLPT);

    return;
}   // end fRun_1DOF_P_BASIC

Here is the call graph for this function:

Here is the caller graph for this function:

void fRun_3DOF_B_BASIC	(	struct SV_3DOF_B_BASIC *	pthisSV,
		struct MagSensor *	pthisMag
	)

Definition at line 517 of file fusion.c.

Referenced by fFuseSensors().

{
    // if requested, do a reset and return
    if (pthisSV->resetflag)
    {
        fInit_3DOF_B_BASIC(pthisSV, pthisMag, FLPFSECS_3DOF_B_BASIC);
        return;
    }

    // calculate the 3DOF magnetometer orientation matrix from fBc
#if THISCOORDSYSTEM == NED
    f3DOFMagnetometerMatrixNED(pthisSV->fR, pthisMag->fBc);
#elif THISCOORDSYSTEM == ANDROID
    f3DOFMagnetometerMatrixAndroid(pthisSV->fR, pthisMag->fBc);
#else // WIN8
    f3DOFMagnetometerMatrixWin8(pthisSV->fR, pthisMag->fBc);
#endif

    //  compute the instantaneous quaternion and low pass filter
    fQuaternionFromRotationMatrix(pthisSV->fR, &(pthisSV->fq));
    fLPFOrientationQuaternion(&(pthisSV->fq), &(pthisSV->fLPq), pthisSV->flpf,
                              pthisSV->fdeltat, pthisSV->fOmega);

    // compute the low pass rotation matrix and rotation vector from low pass filtered quaternion
    fRotationMatrixFromQuaternion(pthisSV->fLPR, &(pthisSV->fLPq));
    fRotationVectorDegFromQuaternion(&(pthisSV->fLPq), pthisSV->fLPRVec);

    // calculate the Euler angles from the low pass orientation matrix
#if THISCOORDSYSTEM == NED
    fNEDAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                    &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                    &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#elif THISCOORDSYSTEM == ANDROID
    fAndroidAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                        &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                        &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#else // WIN8
    fWin8AnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                     &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                     &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#endif
    return;
}

Here is the call graph for this function:

Here is the caller graph for this function:

void fRun_3DOF_G_BASIC	(	struct SV_3DOF_G_BASIC *	pthisSV,
		struct AccelSensor *	pthisAccel
	)

Definition at line 467 of file fusion.c.

Referenced by fFuseSensors().

{
    // if requested, do a reset and return
    if (pthisSV->resetflag)
    {
        fInit_3DOF_G_BASIC(pthisSV, pthisAccel, FLPFSECS_3DOF_G_BASIC);
        return;
    }

    // apply the tilt estimation algorithm to get the instantaneous orientation matrix
#if THISCOORDSYSTEM == NED
    f3DOFTiltNED(pthisSV->fR, pthisAccel->fGc);
#elif THISCOORDSYSTEM == ANDROID
    f3DOFTiltAndroid(pthisSV->fR, pthisAccel->fGc);
#else // WIN8
    f3DOFTiltWin8(pthisSV->fR, pthisAccel->fGc);
#endif

    // compute the instantaneous quaternion and low pass filter
    fQuaternionFromRotationMatrix(pthisSV->fR, &(pthisSV->fq));
    fLPFOrientationQuaternion(&(pthisSV->fq), &(pthisSV->fLPq), pthisSV->flpf,
                              pthisSV->fdeltat, pthisSV->fOmega);

    // compute the low pass rotation matrix and rotation vector from low pass filtered quaternion
    fRotationMatrixFromQuaternion(pthisSV->fLPR, &(pthisSV->fLPq));
    fRotationVectorDegFromQuaternion(&(pthisSV->fLPq), pthisSV->fLPRVec);

    // calculate the Euler angles from the low pass orientation matrix
#if THISCOORDSYSTEM == NED
    fNEDAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                    &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                    &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#elif THISCOORDSYSTEM == ANDROID
    fAndroidAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                        &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                        &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#else // WIN8
    fWin8AnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                     &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                     &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#endif

    // force the yaw and compass angles to zero
    pthisSV->fLPPsi = pthisSV->fLPRho = 0.0F;

    return;
}   // end fRun_3DOF_G_BASIC

Here is the call graph for this function:

Here is the caller graph for this function:

void fRun_3DOF_Y_BASIC	(	struct SV_3DOF_Y_BASIC *	pthisSV,
		struct GyroSensor *	pthisGyro
	)

Definition at line 562 of file fusion.c.

Referenced by fFuseSensors().

{
    Quaternion  ftmpq;  // scratch quaternion
    int8        i;      // loop counter

    // if requested, do a reset and return
    if (pthisSV->resetflag)
    {
        fInit_3DOF_Y_BASIC(pthisSV);
        return;
    }

    // perform an approximate gyro integration for this algorithm using the average of all gyro measurments
    // in the FIFO rather than computing the products of the individual quaternions.
    // the reason is this algorithm does not estimate and subtract the gyro offset so any loss of integration accuracy
    // from using the average gyro measurement is irrelevant.
    // calculate the angular velocity (deg/s) and rotation vector from the average measurement
    for (i = CHX; i <= CHZ; i++) pthisSV->fOmega[i] = pthisGyro->fYs[i];

    // compute the incremental rotation quaternion ftmpq, rotate the orientation quaternion fq
    // and re-normalize fq to prevent error propagation
    fQuaternionFromRotationVectorDeg(&ftmpq, pthisSV->fOmega, pthisSV->fdeltat);
    qAeqAxB(&(pthisSV->fq), &ftmpq);
    fqAeqNormqA(&(pthisSV->fq));

    // get the rotation matrix and rotation vector from the orientation quaternion fq
    fRotationMatrixFromQuaternion(pthisSV->fR, &(pthisSV->fq));
    fRotationVectorDegFromQuaternion(&(pthisSV->fq), pthisSV->fRVec);

    // compute the Euler angles from the orientation matrix
#if THISCOORDSYSTEM == NED
    fNEDAnglesDegFromRotationMatrix(pthisSV->fR, &(pthisSV->fPhi),
                                    &(pthisSV->fThe), &(pthisSV->fPsi),
                                    &(pthisSV->fRho), &(pthisSV->fChi));
#elif THISCOORDSYSTEM == ANDROID
    fAndroidAnglesDegFromRotationMatrix(pthisSV->fR, &(pthisSV->fPhi),
                                        &(pthisSV->fThe), &(pthisSV->fPsi),
                                        &(pthisSV->fRho), &(pthisSV->fChi));
#else // WIN8
    fWin8AnglesDegFromRotationMatrix(pthisSV->fR, &(pthisSV->fPhi),
                                     &(pthisSV->fThe), &(pthisSV->fPsi),
                                     &(pthisSV->fRho), &(pthisSV->fChi));
#endif
    return;
}                       // end fRun_3DOF_Y_BASIC

Here is the call graph for this function:

Here is the caller graph for this function:

void fRun_6DOF_GB_BASIC	(	struct SV_6DOF_GB_BASIC *	pthisSV,
		struct MagSensor *	pthisMag,
		struct AccelSensor *	pthisAccel
	)

Definition at line 610 of file fusion.c.

Referenced by fFuseSensors().

{
    float ftmp1, ftmp2, ftmp3, ftmp4;

    // if requested, do a reset and return
    if (pthisSV->resetflag)
    {
        fInit_6DOF_GB_BASIC(pthisSV, pthisAccel, pthisMag,
                            FLPFSECS_6DOF_GB_BASIC);
        return;
    }

    // call the eCompass algorithm to get the instantaneous orientation matrix and inclination angle
#if THISCOORDSYSTEM == NED
    feCompassNED(pthisSV->fR, &(pthisSV->fDelta), &ftmp1, &ftmp2, pthisMag->fBc, pthisAccel->fGc, &ftmp3, &ftmp4);
#elif THISCOORDSYSTEM == ANDROID
    feCompassAndroid(pthisSV->fR, &(pthisSV->fDelta), &ftmp1, &ftmp2, pthisMag->fBc, pthisAccel->fGc, &ftmp3, &ftmp4);
#else  // WIN8
    feCompassWin8(pthisSV->fR, &(pthisSV->fDelta), &ftmp1, &ftmp2, pthisMag->fBc, pthisAccel->fGc, &ftmp3, &ftmp4);
#endif

    //  compute the instantaneous quaternion and low pass filter
    fQuaternionFromRotationMatrix(pthisSV->fR, &(pthisSV->fq));
    fLPFOrientationQuaternion(&(pthisSV->fq), &(pthisSV->fLPq), pthisSV->flpf,
                              pthisSV->fdeltat, pthisSV->fOmega);

    // compute the low pass rotation matrix and rotation vector from low pass filtered quaternion
    fRotationMatrixFromQuaternion(pthisSV->fLPR, &(pthisSV->fLPq));
    fRotationVectorDegFromQuaternion(&(pthisSV->fLPq), pthisSV->fLPRVec);

    // calculate the Euler angles from the low pass orientation matrix
#if THISCOORDSYSTEM == NED
    fNEDAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                    &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                    &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#elif THISCOORDSYSTEM == ANDROID
    fAndroidAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                        &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                        &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#else // WIN8
    fWin8AnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                     &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                     &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#endif

    // low pass filter the geomagnetic inclination angle with a simple exponential filter
    pthisSV->fLPDelta += pthisSV->flpf * (pthisSV->fDelta - pthisSV->fLPDelta);

    return;
}   // end fRun_6DOF_GB_BASIC

Here is the call graph for this function:

Here is the caller graph for this function:

void fRun_6DOF_GY_KALMAN	(	struct SV_6DOF_GY_KALMAN *	pthisSV,
		struct AccelSensor *	pthisAccel,
		struct GyroSensor *	pthisGyro
	)

Definition at line 663 of file fusion.c.

Referenced by fFuseSensors().

{
    // local scalars and arrays
    float       ftmpMi3x1[3];       // temporary vector used for a priori calculations
    float       ftmp3DOF3x1[3];     // temporary vector used for 3DOF calculations
    float       ftmpA3x3[3][3];     // scratch 3x3 matrix
    float       fQvGQa;             // accelerometer noise covariance to 1g sphere
    float       fC3x6ik;            // element i, k of measurement matrix C
    float       fC3x6jk;            // element j, k of measurement matrix C
    float       fmodGc;             // modulus of fGc[]
    Quaternion  fqMi;               // a priori orientation quaternion
    Quaternion  ftmpq;              // scratch quaternion
    float       ftmp;               // scratch float
    int8        ierror;             // matrix inversion error flag
    int8        i,
                j,
                k;                  // loop counters

    // working arrays for 3x3 matrix inversion
    float       *pfRows[3];
    int8        iColInd[3];
    int8        iRowInd[3];
    int8        iPivot[3];

    // if requested, do a reset initialization with no further processing
    if (pthisSV->resetflag)
    {
        fInit_6DOF_GY_KALMAN(pthisSV, pthisAccel, pthisGyro);
        return;
    }

    // compute the average angular velocity (used for display only) from the average measurement minus gyro offset
    for (i = CHX; i <= CHZ; i++)
        pthisSV->fOmega[i] = (float) pthisGyro->iYs[i] *
            pthisGyro->fDegPerSecPerCount -
            pthisSV->fbPl[i];

    // initialize the a priori orientation quaternion fqMi to the previous iteration's a posteriori estimate
    // and incrementally rotate fqMi by the contents of the gyro FIFO buffer
    fqMi = pthisSV->fqPl;
    if (pthisGyro->iFIFOCount > 0)
    {
        // set ftmp to the interval between the FIFO gyro measurements
        ftmp = pthisSV->fdeltat / (float) pthisGyro->iFIFOCount;

        // normal case, loop over all the buffered gyroscope measurements
        for (j = 0; j < pthisGyro->iFIFOCount; j++)
        {
            // calculate the instantaneous angular velocity subtracting the gyro offset
            for (i = CHX; i <= CHZ; i++)
                ftmpMi3x1[i] = (float) pthisGyro->iYsFIFO[j][i] *
                    pthisGyro->fDegPerSecPerCount -
                    pthisSV->fbPl[i];

            // compute the incremental rotation quaternion ftmpq and integrate the a priori orientation quaternion fqMi
            fQuaternionFromRotationVectorDeg(&ftmpq, ftmpMi3x1, ftmp);
            qAeqAxB(&fqMi, &ftmpq);
        }
    }
    else
    {
        // special case with no new FIFO measurements, use the previous iteration's average gyro reading to compute
        // the incremental rotation quaternion ftmpq and integrate the a priori orientation quaternion fqMi
        fQuaternionFromRotationVectorDeg(&ftmpq, pthisSV->fOmega,
                                         pthisSV->fdeltat);
        qAeqAxB(&fqMi, &ftmpq);
    }

    // set ftmp3DOF3x1 to the 3DOF gravity vector in the sensor frame
    fmodGc = sqrtf(fabs(pthisAccel->fGc[CHX] * pthisAccel->fGc[CHX] +
                   pthisAccel->fGc[CHY] * pthisAccel->fGc[CHY] +
                   pthisAccel->fGc[CHZ] * pthisAccel->fGc[CHZ]));
    if (fmodGc != 0.0F)
    {
        // normal non-freefall case
        ftmp = 1.0F / fmodGc;
        ftmp3DOF3x1[CHX] = pthisAccel->fGc[CHX] * ftmp;
        ftmp3DOF3x1[CHY] = pthisAccel->fGc[CHY] * ftmp;
        ftmp3DOF3x1[CHZ] = pthisAccel->fGc[CHZ] * ftmp;
    }
    else
    {
        // use zero tilt in case of freefall
        ftmp3DOF3x1[CHX] = 0.0F;
        ftmp3DOF3x1[CHY] = 0.0F;
        ftmp3DOF3x1[CHZ] = 1.0F;
    }

    // correct accelerometer gravity vector for different coordinate systems
#if THISCOORDSYSTEM == NED
    // +1g in accelerometer z axis (z down) when PCB is flat so no correction needed
#elif THISCOORDSYSTEM == ANDROID
    // +1g in accelerometer z axis (z up) when PCB is flat so negate the vector to obtain gravity
    ftmp3DOF3x1[CHX] = -ftmp3DOF3x1[CHX];
    ftmp3DOF3x1[CHY] = -ftmp3DOF3x1[CHY];
    ftmp3DOF3x1[CHZ] = -ftmp3DOF3x1[CHZ];
#else // WIN8
    // -1g in accelerometer z axis (z up) when PCB is flat so no correction needed
#endif

    // set ftmpMi3x1 to the a priori gravity vector in the sensor frame from the a priori quaternion
    ftmpMi3x1[CHX] = 2.0F * (fqMi.q1 * fqMi.q3 - fqMi.q0 * fqMi.q2);
    ftmpMi3x1[CHY] = 2.0F * (fqMi.q2 * fqMi.q3 + fqMi.q0 * fqMi.q1);
    ftmpMi3x1[CHZ] = 2.0F * (fqMi.q0 * fqMi.q0 + fqMi.q3 * fqMi.q3) - 1.0F;

    // correct a priori gravity vector for different coordinate systems
#if THISCOORDSYSTEM == NED
    // z axis is down (NED) so no correction needed
#else // ANDROID and WIN8
    // z axis is up (ANDROID and WIN8 ENU) so no negate the vector to obtain gravity
    ftmpMi3x1[CHX] = -ftmpMi3x1[CHX];
    ftmpMi3x1[CHY] = -ftmpMi3x1[CHY];
    ftmpMi3x1[CHZ] = -ftmpMi3x1[CHZ];
#endif

    // calculate the rotation quaternion between 3DOF and a priori gravity vectors (ignored minus signs cancel here)
    // and copy the quaternion vector components to the measurement error vector fZErr
    fveqconjgquq(&ftmpq, ftmp3DOF3x1, ftmpMi3x1);
    pthisSV->fZErr[CHX] = ftmpq.q1;
    pthisSV->fZErr[CHY] = ftmpq.q2;
    pthisSV->fZErr[CHZ] = ftmpq.q3;

    // update Qw using the a posteriori error vectors from the previous iteration.
    // as Qv increases or Qw decreases, K -> 0 and the Kalman filter is weighted towards the a priori prediction
    // as Qv decreases or Qw increases, KC -> I and the Kalman filter is weighted towards the measurement.
    // initialize Qw to all zeroes
    for (i = 0; i < 6; i++)
        for (j = 0; j < 6; j++) pthisSV->fQw6x6[i][j] = 0.0F;

    // partial diagonal gyro offset terms
    pthisSV->fQw6x6[3][3] = pthisSV->fbErrPl[CHX] * pthisSV->fbErrPl[CHX];
    pthisSV->fQw6x6[4][4] = pthisSV->fbErrPl[CHY] * pthisSV->fbErrPl[CHY];
    pthisSV->fQw6x6[5][5] = pthisSV->fbErrPl[CHZ] * pthisSV->fbErrPl[CHZ];

    // diagonal gravity vector components
    pthisSV->fQw6x6[0][0] = pthisSV->fqgErrPl[CHX] *
        pthisSV->fqgErrPl[CHX] +
        pthisSV->fAlphaSqQvYQwbOver12 +
        pthisSV->fAlphaSqOver4 *
        pthisSV->fQw6x6[3][3];
    pthisSV->fQw6x6[1][1] = pthisSV->fqgErrPl[CHY] *
        pthisSV->fqgErrPl[CHY] +
        pthisSV->fAlphaSqQvYQwbOver12 +
        pthisSV->fAlphaSqOver4 *
        pthisSV->fQw6x6[4][4];
    pthisSV->fQw6x6[2][2] = pthisSV->fqgErrPl[CHZ] *
        pthisSV->fqgErrPl[CHZ] +
        pthisSV->fAlphaSqQvYQwbOver12 +
        pthisSV->fAlphaSqOver4 *
        pthisSV->fQw6x6[5][5];

    // remaining diagonal gyro offset components
    pthisSV->fQw6x6[3][3] += pthisSV->fQwbOver3;
    pthisSV->fQw6x6[4][4] += pthisSV->fQwbOver3;
    pthisSV->fQw6x6[5][5] += pthisSV->fQwbOver3;

    // off diagonal gravity and gyro offset components
    pthisSV->fQw6x6[0][3] = pthisSV->fQw6x6[3][0] = pthisSV->fqgErrPl[CHX] *
        pthisSV->fbErrPl[CHX] -
        pthisSV->fAlphaOver2 *
        pthisSV->fQw6x6[3][3];
    pthisSV->fQw6x6[1][4] = pthisSV->fQw6x6[4][1] = pthisSV->fqgErrPl[CHY] *
        pthisSV->fbErrPl[CHY] -
        pthisSV->fAlphaOver2 *
        pthisSV->fQw6x6[4][4];
    pthisSV->fQw6x6[2][5] = pthisSV->fQw6x6[5][2] = pthisSV->fqgErrPl[CHZ] *
        pthisSV->fbErrPl[CHZ] -
        pthisSV->fAlphaOver2 *
        pthisSV->fQw6x6[5][5];

    // calculate the vector fQv containing the diagonal elements of the measurement covariance matrix Qv
    ftmp = fmodGc - 1.0F;
    fQvGQa = 3.0F * ftmp * ftmp;
    if (fQvGQa < FQVG_6DOF_GY_KALMAN) fQvGQa = FQVG_6DOF_GY_KALMAN;
    pthisSV->fQv = ONEOVER12 * ftmp * ftmp + pthisSV->fAlphaSqQvYQwbOver12;

    // calculate the 6x3 Kalman gain matrix K = Qw * C^T * inv(C * Qw * C^T + Qv)
    // set fQwCT6x3 = Qw.C^T where Qw has size 6x6 and C^T has size 6x3
    for (i = 0; i < 6; i++)         // loop over rows
    {
        for (j = 0; j < 3; j++)     // loop over columns
        {
            pthisSV->fQwCT6x3[i][j] = 0.0F;

            // accumulate matrix sum
            for (k = 0; k < 6; k++)
            {
                // determine fC3x6[j][k] since the matrix is highly sparse
                fC3x6jk = 0.0F;
                if (k == j) fC3x6jk = 1.0F;
                if (k == (j + 3)) fC3x6jk = -pthisSV->fAlphaOver2;

                // accumulate fQwCT6x3[i][j] += Qw6x6[i][k] * C[j][k]
                if ((pthisSV->fQw6x6[i][k] != 0.0F) && (fC3x6jk != 0.0F))
                {
                    if (fC3x6jk == 1.0F)
                        pthisSV->fQwCT6x3[i][j] += pthisSV->fQw6x6[i][k];
                    else
                        pthisSV->fQwCT6x3[i][j] += pthisSV->fQw6x6[i][k] * fC3x6jk;
                }
            }
        }
    }

    // set symmetric ftmpA3x3 = C.(Qw.C^T) + Qv = C.fQwCT6x3 + Qv
    for (i = 0; i < 3; i++)         // loop over rows
    {
        for (j = i; j < 3; j++)     // loop over on and above diagonal columns
        {
            // zero off diagonal and set diagonal to Qv
            if (i == j)
                ftmpA3x3[i][j] = pthisSV->fQv;
            else
                ftmpA3x3[i][j] = 0.0F;

            // accumulate matrix sum
            for (k = 0; k < 6; k++)
            {
                // determine fC3x6[i][k]
                fC3x6ik = 0.0F;
                if (k == i) fC3x6ik = 1.0F;
                if (k == (i + 3)) fC3x6ik = -pthisSV->fAlphaOver2;

                // accumulate ftmpA3x3[i][j] += C[i][k] & fQwCT6x3[k][j]
                if ((fC3x6ik != 0.0F) && (pthisSV->fQwCT6x3[k][j] != 0.0F))
                {
                    if (fC3x6ik == 1.0F)
                        ftmpA3x3[i][j] += pthisSV->fQwCT6x3[k][j];
                    else
                        ftmpA3x3[i][j] += fC3x6ik * pthisSV->fQwCT6x3[k][j];
                }
            }
        }
    }

    // set ftmpA3x3 below diagonal elements to above diagonal elements
    ftmpA3x3[1][0] = ftmpA3x3[0][1];
    ftmpA3x3[2][0] = ftmpA3x3[0][2];
    ftmpA3x3[2][1] = ftmpA3x3[1][2];

    // invert ftmpA3x3 in situ to give ftmpA3x3 = inv(C * Qw * C^T + Qv) = inv(ftmpA3x3)
    for (i = 0; i < 3; i++) pfRows[i] = ftmpA3x3[i];
    fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 3, &ierror);

    // on successful inversion set Kalman gain matrix fK6x3 = Qw * C^T * inv(C * Qw * C^T + Qv) = fQwCT6x3 * ftmpA3x3
    if (!ierror)
    {
        // normal case
        for (i = 0; i < 6; i++)     // loop over rows
        {
            for (j = 0; j < 3; j++) // loop over columns
            {
                pthisSV->fK6x3[i][j] = 0.0F;
                for (k = 0; k < 3; k++)
                {
                    if ((pthisSV->fQwCT6x3[i][k] != 0.0F) &&
                        (ftmpA3x3[k][j] != 0.0F))
                    {
                        pthisSV->fK6x3[i][j] += pthisSV->fQwCT6x3[i][k] * ftmpA3x3[k][j];
                    }
                }
            }
        }
    }
    else
    {
        // ftmpA3x3 was singular so set Kalman gain matrix fK6x3 to zero
        for (i = 0; i < 6; i++)     // loop over rows
        {
            for (j = 0; j < 3; j++) // loop over columns
            {
                pthisSV->fK6x3[i][j] = 0.0F;
            }
        }
    }

    // calculate the a posteriori gravity and geomagnetic tilt quaternion errors and gyro offset error vector
    // from the Kalman matrix fK6x3 and from the measurement error vector fZErr.
    for (i = CHX; i <= CHZ; i++)
    {
        // gravity tilt vector error component
        if (pthisSV->fK6x3[i][CHX] != 0.0F)
            pthisSV->fqgErrPl[i] = pthisSV->fK6x3[i][CHX] * pthisSV->fZErr[CHX];
        else
            pthisSV->fqgErrPl[i] = 0.0F;
        if (pthisSV->fK6x3[i][CHY] != 0.0F)
            pthisSV->fqgErrPl[i] += pthisSV->fK6x3[i][CHY] * pthisSV->fZErr[CHY];
        if (pthisSV->fK6x3[i][CHZ] != 0.0F)
            pthisSV->fqgErrPl[i] += pthisSV->fK6x3[i][CHZ] * pthisSV->fZErr[CHZ];

        // gyro offset vector error component
        if (pthisSV->fK6x3[i + 3][CHX] != 0.0F)
            pthisSV->fbErrPl[i] = pthisSV->fK6x3[i + 3][CHX] * pthisSV->fZErr[CHX];
        else
            pthisSV->fbErrPl[i] = 0.0F;
        if (pthisSV->fK6x3[i + 3][CHY] != 0.0F)
            pthisSV->fbErrPl[i] += pthisSV->fK6x3[i + 3][CHY] * pthisSV->fZErr[CHY];
        if (pthisSV->fK6x3[i + 3][CHZ] != 0.0F)
            pthisSV->fbErrPl[i] += pthisSV->fK6x3[i + 3][CHZ] * pthisSV->fZErr[CHZ];
    }

    // set ftmpq to the gravity tilt correction (conjugate) quaternion
    ftmpq.q1 = -pthisSV->fqgErrPl[CHX];
    ftmpq.q2 = -pthisSV->fqgErrPl[CHY];
    ftmpq.q3 = -pthisSV->fqgErrPl[CHZ];
    ftmp = ftmpq.q1 * ftmpq.q1 + ftmpq.q2 * ftmpq.q2 + ftmpq.q3 * ftmpq.q3;

    // determine the scalar component q0 to enforce normalization
    if (ftmp <= 1.0F)
    {
        // normal case
        ftmpq.q0 = sqrtf(fabsf(1.0F - ftmp));
    }
    else
    {
        // if vector component exceeds unity then set to 180 degree rotation and force normalization
        ftmp = 1.0F / sqrtf(ftmp);
        ftmpq.q0 = 0.0F;
        ftmpq.q1 *= ftmp;
        ftmpq.q2 *= ftmp;
        ftmpq.q3 *= ftmp;
    }

    // apply the gravity tilt correction quaternion so fqPl = fqMi.(fqgErrPl)* = fqMi.ftmpq and normalize
    qAeqBxC(&(pthisSV->fqPl), &fqMi, &ftmpq);

    // normalize the a posteriori quaternion and compute the a posteriori rotation matrix and rotation vector
    fqAeqNormqA(&(pthisSV->fqPl));
    fRotationMatrixFromQuaternion(pthisSV->fRPl, &(pthisSV->fqPl));
    fRotationVectorDegFromQuaternion(&(pthisSV->fqPl), pthisSV->fRVecPl);

    // update the a posteriori gyro offset vector: b+[k] = b-[k] - be+[k] = b+[k] - be+[k] (deg/s)
    // limiting the correction to the maximum permitted by the random walk model
    for (i = CHX; i <= CHZ; i++)
    {
        if (pthisSV->fbErrPl[i] > pthisSV->fMaxGyroOffsetChange)
            pthisSV->fbPl[i] -= pthisSV->fMaxGyroOffsetChange;
        else if (pthisSV->fbErrPl[i] < -pthisSV->fMaxGyroOffsetChange)
            pthisSV->fbPl[i] += pthisSV->fMaxGyroOffsetChange;
        else
            pthisSV->fbPl[i] -= pthisSV->fbErrPl[i];
    }

    // compute the linear acceleration fAccGl in the global frame
    // first de-rotate the accelerometer measurement fGc from the sensor to global frame
    // using the transpose (inverse) of the orientation matrix fRPl
    fveqRu(pthisSV->fAccGl, pthisSV->fRPl, pthisAccel->fGc, 1);

    // sutract the fixed gravity vector in the global frame leaving linear acceleration
#if THISCOORDSYSTEM == NED
    // gravity positive NED
    pthisSV->fAccGl[CHX] = -pthisSV->fAccGl[CHX];
    pthisSV->fAccGl[CHY] = -pthisSV->fAccGl[CHY];
    pthisSV->fAccGl[CHZ] = -(pthisSV->fAccGl[CHZ] - 1.0F);
#elif THISCOORDSYSTEM == ANDROID
    // acceleration positive ENU
    pthisSV->fAccGl[CHZ] = pthisSV->fAccGl[CHZ] - 1.0F;
#else // WIN8
    // gravity positive ENU
    pthisSV->fAccGl[CHX] = -pthisSV->fAccGl[CHX];
    pthisSV->fAccGl[CHY] = -pthisSV->fAccGl[CHY];
    pthisSV->fAccGl[CHZ] = -(pthisSV->fAccGl[CHZ] + 1.0F);
#endif

    // compute the a posteriori Euler angles from the a posteriori orientation matrix fRPl
#if THISCOORDSYSTEM == NED
    fNEDAnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl),
                                    &(pthisSV->fThePl), &(pthisSV->fPsiPl),
                                    &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
#elif THISCOORDSYSTEM == ANDROID
    fAndroidAnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl),
                                        &(pthisSV->fThePl), &(pthisSV->fPsiPl),
                                        &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
#else // WIN8
    fWin8AnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl),
                                     &(pthisSV->fThePl), &(pthisSV->fPsiPl),
                                     &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
#endif
    return;
}   // end fRun_6DOF_GY_KALMAN

Here is the call graph for this function:

Here is the caller graph for this function:

void fRun_9DOF_GBY_KALMAN	(	struct SV_9DOF_GBY_KALMAN *	pthisSV,
		struct AccelSensor *	pthisAccel,
		struct MagSensor *	pthisMag,
		struct GyroSensor *	pthisGyro,
		struct MagCalibration *	pthisMagCal
	)

Referenced by fFuseSensors(), and fRun_6DOF_GY_KALMAN().

Here is the caller graph for this function: